#include "control.h"
#include "usart.h"


int PWM_Out_One,PWM_Out_Two;

float X_Kp = 0.35,//0.3
	X_Kd = 20;//20

float Y_Kp = -0.35,//0.4
	Y_Kd = -35;// -35



void PID(float Angel_X,float Angel_Y,float target_x,float target_y)
{
	static float X_Cha_Last,Y_Cha_Last,X_Last,Y_Last,X_Cha,Y_Cha,X_D,Y_D;
	
	Y_Cha_Last = Angel_Y - target_y;
	Y_D = (Y_Cha_Last - Y_Cha);
	Y_Cha = Y_Cha_Last;
	PWM_Out_Two = Y_Kp * Y_Cha_Last + Y_Kd * Y_D;
	
	
	X_Cha_Last = Angel_X - target_x;
	X_D = (X_Cha_Last - X_Cha);
	X_Cha = X_Cha_Last;
	PWM_Out_One = X_Kp * X_Cha_Last + X_Kd * X_D;
	
	
}



int GFP_abs(int p)
{
	int q;
	q=p>0?p:(-p);
	return q;
}


void Load(u16 motor1,u16 motor2)
{
	TIM_SetCompare1(TIM3,GFP_abs(motor1 + flage1));
	TIM_SetCompare2(TIM3,GFP_abs(motor2 + flage2));
}







